// MIT License
//
// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
// Stachniss.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include <Eigen/Core>
#include <memory>
#include <utility>
#include <vector>

// KISS-ICP-ROS
#include "OdometryServer.hpp"
#include "Utils.hpp"

// KISS-ICP
// #include "kiss_icp/pipeline/KissICP.hpp"
#include "OdometryServer.hpp"
#include "../../dds/perception_msgsPubSubTypes.h"
#include "../../dds/fusion_localizationPubSubTypes.h"


// ROS 1 headers
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/init.h>
#include <ros/node_handle.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <iostream>
#include <fstream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>



namespace kiss_icp_ros {

using utils::EigenToPointCloud2;
using utils::GetTimestamps;
using utils::PointCloud2ToEigen;




OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh)
    : nh_(nh), pnh_(pnh), tf2_listener_(tf2_ros::TransformListener(tf2_buffer_)) {
    pnh_.param("base_frame", base_frame_, base_frame_);
    pnh_.param("odom_frame", odom_frame_, odom_frame_);
    pnh_.param("publish_odom_tf", publish_odom_tf_, false);
    pnh_.param("visualize", publish_debug_clouds_, publish_debug_clouds_);
    pnh_.param("max_range", config_.max_range, 100.0);
    pnh_.param("min_range", config_.min_range, 5.0);
    pnh_.param("deskew", config_.deskew, false);
    pnh_.param("voxel_size", config_.voxel_size, config_.max_range / 100.0);
    pnh_.param("max_points_per_voxel", config_.max_points_per_voxel, 20);
    pnh_.param("initial_threshold", config_.initial_threshold, 2.0);
    pnh_.param("min_motion_th", config_.min_motion_th, 0.1);
    if (config_.max_range < config_.min_range) {
        ROS_WARN("[WARNING] max_range is smaller than min_range, setting min_range to 0.0");
        config_.min_range = 0.0;
    }

    // Construct the main KISS-ICP odometry node
    odometry_ = kiss_icp::pipeline::KissICP(config_);

    // Initialize subscribers
    // pointcloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("lidar_front/raw_cloud", queue_size_,
    //                                                           &OdometryServer::RegisterFrame, this);
    std::string pointCloudTopic = "/velodyne_points";
    std::string odome_incremental = "lio_sam/mapping/odometry_incremental";


    cloud_msg = new DDS_LidarCloudPubSubType();
    odom_msg = new DDS_OdometryPubSubType();

    dds_laser_cloud_ = new ddsSubscriber<DDS_LidarCloud>(pointCloudTopic.c_str(), cloud_msg, std::bind(&OdometryServer::ddsCloudHandler, this, std::placeholders::_1));

    dds_odom_pub_ = new ddsPublisher<DDS_Odometry>(odome_incremental.c_str(), odom_msg);

    // Initialize publishers
    odom_publisher_ = pnh_.advertise<nav_msgs::Odometry>("/kiss/odometry", queue_size_);
    traj_publisher_ = pnh_.advertise<nav_msgs::Path>("/kiss/trajectory", queue_size_);
    if (publish_debug_clouds_) {
        frame_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("/kiss/frame", queue_size_);
        kpoints_publisher_ =
            pnh_.advertise<sensor_msgs::PointCloud2>("/kiss/keypoints", queue_size_);
        map_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("/kiss/local_map", queue_size_);
    }
    // Initialize the transform buffer
    tf2_buffer_.setUsingDedicatedThread(true);
    path_msg_.header.frame_id = odom_frame_;

    // publish odometry msg
    ROS_INFO("KISS-ICP ROS 1 Odometry Node Initialized");
}

void OdometryServer::pointToEigen(const DDS_LidarCloud &laserCloudMsg, std::vector<Eigen::Vector3d> &res_points, std::vector<double> &res_times)
{
    for (auto point : laserCloudMsg.points())
    {
        res_times.push_back(point.time_stamp());
        res_points.push_back(Eigen::Vector3d(double(point.point_x()), double(point.point_y()), double(point.point_z())));
    }
}

bool OdometryServer::SaveTumPose(std::string save_path, DDS_Odometry &pub_odom_data)
    {
        std::ofstream fout(save_path, std::ofstream::app);
        if (fout)
        {
            fout << std::setprecision(18) << pub_odom_data.timestamp() << " "
                 << pub_odom_data.x() << " "
                 << pub_odom_data.y() << " "
                 << pub_odom_data.z() << " "
                 << pub_odom_data.qx() << " "
                 << pub_odom_data.qy() << " "
                 << pub_odom_data.qz() << " "
                 << pub_odom_data.qw() << std::endl;
            return true;
        }
        return false;
    }

void OdometryServer::ddsCloudHandler(const DDS_LidarCloud &laserCloudMsg)
{
    // const auto cloud_frame_id = msg->header.frame_id;

    std::vector<Eigen::Vector3d> points;
    std::vector<double> timestamps;

    pointToEigen(laserCloudMsg, points, timestamps);

    // const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
    const auto egocentric_estimation = false;

    // Register frame, main entry point to KISS-ICP pipeline
    const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps);

    // Compute the pose using KISS, ego-centric to the LiDAR
    const Sophus::SE3d kiss_pose = odometry_.poses().back();
    std::cout << "pose: "  << std::endl << kiss_pose.matrix() << std::endl;


    DDS_Odometry pub_odom_data;
    pub_odom_data.timestamp() = double(laserCloudMsg.stamp() / 1000.0);
    pub_odom_data.x() = kiss_pose.translation()[0];
    pub_odom_data.y() = kiss_pose.translation()[1];
    pub_odom_data.z() = kiss_pose.translation()[2];

    Eigen::Quaterniond laser_odom_q = Eigen::Quaterniond(kiss_pose.rotationMatrix().matrix());
    pub_odom_data.qw() = laser_odom_q.w();
    pub_odom_data.qx() = laser_odom_q.x();
    pub_odom_data.qy() = laser_odom_q.y();
    pub_odom_data.qz() = laser_odom_q.z();
    pub_odom_data.odom_status() = 0;

    SaveTumPose("../gtsam_odom.txt", pub_odom_data);

    dds_odom_pub_->publish(pub_odom_data);

    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
    // const auto pose = [&]() -> Sophus::SE3d {
    //     if (egocentric_estimation) return kiss_pose;
    //     const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
    //     return cloud2base * kiss_pose * cloud2base.inverse();
    // }();

    // readPose();
    // savePose(pose, msg->header.stamp.toSec());

    // saveCloud(frame, msg->header.stamp.toSec());

    // Spit the current estimated pose to ROS msgs
    // PublishOdometry(pose, msg->header.stamp, cloud_frame_id);

    // Publishing this clouds is a bit costly, so do it only if we are debugging
    // if (publish_debug_clouds_) {
    //     PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id);
    // }
}

Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame,
                                             const std::string &source_frame) const {
    std::string err_msg;
    if (tf2_buffer_._frameExists(source_frame) &&  //
        tf2_buffer_._frameExists(target_frame) &&  //
        tf2_buffer_.canTransform(target_frame, source_frame, ros::Time(0), &err_msg)) {
        try {
            auto tf = tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0));
            return tf2::transformToSophus(tf);
        } catch (tf2::TransformException &ex) {
            ROS_WARN("%s", ex.what());
        }
    }
    ROS_WARN("Failed to find tf between %s and %s. Reason=%s", target_frame.c_str(),
             source_frame.c_str(), err_msg.c_str());
    return {};
}

void OdometryServer::savePose(const Sophus::SE3d& cur_pose, const double time)
{
    std::ofstream file("/home/zmc/projects/kiss_icp_ws/src/kiss-icp/save/data.txt", std::ios::app);
    if (file.is_open())
    {
        // 写入SE3d数据
        file << "SE3d:\n";
        file << cur_pose.matrix() << std::endl;

        // 写入时间
        file << "Time:\n";
        file << std::setprecision(20) << time << std::endl;

        file.close();
        std::cout << "Data saved successfully!" << std::endl;
    }
    else
    {
        std::cerr << "Failed to open file!" << std::endl;
        return;
    }
}

void OdometryServer::readPose()
{
    std::cout << "readpose in" << std::endl;
    std::ifstream file("/home/zmc/projects/kiss_icp_ws/src/kiss-icp/save/data.txt", std::ios::app);
    if (file.is_open())
    {
        // 读取SE3d数据
        std::string line;
        while (std::getline(file, line))
        {
            if (line == "SE3d:")
            {
                std::cout << "readpose in SE3d" << std::endl;
                
                Eigen::Matrix4d matrix;
                for (int i = 0; i < 4; ++i)
                {
                    std::getline(file, line);
                    std::istringstream iss(line);
                    for (int j = 0; j < 4; ++j)
                    {
                        iss >> matrix(i, j);
                    }
                }
                // Sophus::SE3d se3(matrix);
                std::cout << "-------SE3d: " << std::endl;
                std::cout << matrix << std::endl;
                // std::cout << se3.matrix() << std::endl;
            }
            else if (line == "Time:")
            {
                std::getline(file, line);
                std::cout << "------Time: " << std::endl;
                std::cout << std::setprecision(16) << line << std::endl;
            }
        }

        file.close();
    }
    else
    {
        std::cerr << "Failed to open file!" << std::endl;
        return;
    }
}

void OdometryServer::saveCloud(const std::vector<Eigen::Vector3d>& points, const double time)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 将std::vector中的点添加到PCL点云对象
    for (const auto& point : points)
    {
        pcl::PointXYZ pclPoint;
        pclPoint.x = point[0];
        pclPoint.y = point[1];
        pclPoint.z = point[2];
        cloud->push_back(pclPoint);
    }

    // 存储PCL点云为PCD文件
    pcl::io::savePCDFileASCII("/home/zmc/projects/kiss_icp_ws/src/kiss-icp/save/" + std::to_string(time) + ".pcd", *cloud);
    std::cout << "Point cloud saved successfully!" << std::endl;
}

void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg) {
    const auto cloud_frame_id = msg->header.frame_id;
    const auto points = PointCloud2ToEigen(msg);
    const auto timestamps = [&]() -> std::vector<double> {
        if (!config_.deskew) return {};
        return GetTimestamps(msg);
    }();
    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);

    // Register frame, main entry point to KISS-ICP pipeline
    const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps);

    // Compute the pose using KISS, ego-centric to the LiDAR
    const Sophus::SE3d kiss_pose = odometry_.poses().back();

    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
    const auto pose = [&]() -> Sophus::SE3d {
        if (egocentric_estimation) return kiss_pose;
        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
        return cloud2base * kiss_pose * cloud2base.inverse();
    }();

    // readPose();
    savePose(pose, msg->header.stamp.toSec());

    // saveCloud(frame, msg->header.stamp.toSec());

    // Spit the current estimated pose to ROS msgs
    PublishOdometry(pose, msg->header.stamp, cloud_frame_id);

    // Publishing this clouds is a bit costly, so do it only if we are debugging
    if (publish_debug_clouds_) {
        PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id);
    }
}

void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
                                     const ros::Time &stamp,
                                     const std::string &cloud_frame_id) {
    // Header for point clouds and stuff seen from desired odom_frame

    // Broadcast the tf
    if (publish_odom_tf_) {
        geometry_msgs::TransformStamped transform_msg;
        transform_msg.header.stamp = stamp;
        transform_msg.header.frame_id = odom_frame_;
        transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_;
        transform_msg.transform = tf2::sophusToTransform(pose);
        tf_broadcaster_.sendTransform(transform_msg);
    }

    // publish trajectory msg
    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header.stamp = stamp;
    pose_msg.header.frame_id = odom_frame_;
    pose_msg.pose = tf2::sophusToPose(pose);
    path_msg_.poses.push_back(pose_msg);
    traj_publisher_.publish(path_msg_);

    // publish odometry msg
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = stamp;
    odom_msg.header.frame_id = odom_frame_;
    odom_msg.pose.pose = tf2::sophusToPose(pose);
    odom_publisher_.publish(odom_msg);
}

void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
                                   const std::vector<Eigen::Vector3d> keypoints,
                                   const ros::Time &stamp,
                                   const std::string &cloud_frame_id) {
    std_msgs::Header odom_header;
    odom_header.stamp = stamp;
    odom_header.frame_id = odom_frame_;

    // Publish map
    const auto kiss_map = odometry_.LocalMap();

    if (!publish_odom_tf_) {
        // debugging happens in an egocentric world
        std_msgs::Header cloud_header;
        cloud_header.stamp = stamp;
        cloud_header.frame_id = cloud_frame_id;

        frame_publisher_.publish(*EigenToPointCloud2(frame, cloud_header));
        kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud_header));
        map_publisher_.publish(*EigenToPointCloud2(kiss_map, odom_header));

        return;
    }

    // If transmitting to tf tree we know where the clouds are exactly
    const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id);
    frame_publisher_.publish(*EigenToPointCloud2(frame, cloud2odom, odom_header));
    kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud2odom, odom_header));

    if (!base_frame_.empty()) {
        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
        map_publisher_.publish(*EigenToPointCloud2(kiss_map, cloud2base, odom_header));
    } else {
        map_publisher_.publish(*EigenToPointCloud2(kiss_map, odom_header));
    }
}

}  // namespace kiss_icp_ros

static volatile int keepRunning = 1;

void sig_handler(int sig)
{
    if (sig == SIGINT)
    {
        keepRunning = 0;
    }
}


int main(int argc, char **argv) {
    ros::init(argc, argv, "kiss_icp");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");

    kiss_icp_ros::OdometryServer node(nh, nh_private);

    // while (keepRunning)
    // {
    //     sleep(1);
    // }

    ros::spin();

    return 0;
}
